%% Tube MPC scheme based on robust control invariant set with application to Lipschitz nonlinear systems (2013)
% Tube-based NMPC by Zehua Jia, jiazehua@sjtu.edu.cn
% Simulation reproduction of Yu et. al 2013.

%% Tips:
% 首先运行SDPT3-4.0下面的startup.m,否则提示找不到Solver

%% System description
% x' = A * x + g(x) + B * u + Bw * w

%% Initiallization
clear all % 清空所有变量
close all % 关闭所有图形窗口
clc % 清空命令行

% rng(0); % fix random seed
%% Model description
A= [-1, 2; -3, 4]; % 系统矩阵
B = [0.5; -2]; % 输入矩阵
Bw = [0; 1]; % 扰动输入矩阵
% g(x) = [0; 0.25 * x2^3]; % 非线性项

%% Disturbance
wmax = 0.1; % 扰动的最大值
W_vertex = [-wmax, 0; wmax, 0]; % 扰动集合的顶点
% 构建Polyhedron对象，可以使用 MATLAB 工具箱中提供的方法和函数对扰动集合进行各种操作，例如计算多面体的体积、绘制多面体的图形等。
% 在这个特定的代码中，W 表示一个一维的线段，表示在 x 轴上的扰动范围
W = Polyhedron(W_vertex); % 构造扰动集合的Polyhedron对象

%% Problem formulation
dt = 0.1; % 采样周期
N = 15; % 预测时域 N * dt
T = 100; % 仿真时长 T * dt

Q = diag([0.5, 0.5]); % 状态权重矩阵
R = 1; % 输入权重矩阵

%% Initial settings
x0 = [3.5; -2.5]; % 初始状态 x0
% 对于线性定标不等式LDI中的集合M，控制输入u的约束条件
% M为状态空间中的一些限制，例如状态的范围、速度的上限等。控制输入必须在这些限制下保持系统的稳定性和性能
% 管道型MPC中，调整这些集合可以影响管道的宽度和形状
uM = 2; 
% 状态x在集合M上的约束
xM = 2; 
% alphaM被用作名义MPC中终端区域和终端权重的上界，通过调整这个参数，可以尝试不同的管道型MPC设计，以满足不同的性能和稳定性要求
% 这个参数与控制输入u的限制相关
alphaM = 10; % alphaM是alpha的上界，-1表示无上界。

% 定义存储实际状态和控制输入的变量
xc = zeros(2, T); % 存储实际状态
uc = zeros(1, T);
xn = zeros(2, T); % 存储名义状态
un = zeros(1, T);
tilde_x = zeros(2, T); % 存储实际状态-名义状态的差
xc(:, 1) = x0;
xn(:, 1) = x0;
w = zeros(1, T); %存储扰动

%% Caculate the robust invariant set (RIS)=鲁棒控制不变集
% 三个输入参数，分别是系统矩阵A、输入矩阵B和扰动输入矩阵Bw。
% P:鲁棒不变集的某种表示或描述。
% K:标称系统的反馈控制率
% lambda0:用于计算鲁棒不变集的某种参数。
% mu:用于计算鲁棒不变集的某种参数。
[P, K, lambda0, mu] = NMPC_get_RIS (A, B, Bw); 
% lambda = lambda0 - 1; 

%实际应用中，这些参数通常是通过优化算法来得到的，以满足系统的性能和稳定性要求
% 表示在鲁棒控制器中的状态反馈矩阵，用于计算控制输入。
K1 = [-1.3693, 5.1273]; % 论文中的结果
% 对角矩阵，表示鲁棒不变集的形状
P1 = diag([39.0251, 486.0402]); % 论文中的结果

% 可视化鲁棒不变集（Robust Invariant Set）的形状
% 鲁棒不变集是一种表示系统在存在扰动的情况下，状态能够保持在一定范围内的集合。
% P是一个矩阵，表示鲁棒不变集的形状。这个矩阵通常是通过优化算法计算得到的。
% mu*wmax^2/lambda0 表示椭圆的半长轴长度，其中mu是鲁棒性的界限，wmax是扰动的最大值，lambda0是优化问题中的一个参数。

% 图1: 鲁棒不变集的形状
draw_ellip(P, mu * wmax^2/lambda0, 'g') % 绘制椭圆
hold on
% P1是一个矩阵，表示另一个鲁棒不变集的形状。
% 1表示椭圆的半长轴长度为1，这里是手动设置的一个值。
% 'r'是指定绘制的颜色，这里是红色。
draw_ellip(P1, 1, 'r')
% Omega = Polyhedron([-0.1127,-0.04581; -0.1127,0.04581; 0.1126,-0.04581; 0.1126,0.04581]);

%% Calculate the Minkowski difference set
% Minkowski差集是两个集合的一种几何运算，表示其中一个集合中的每个点分别与另一个集合中的所有点之差的集合
% 用于计算控制输入u在鲁棒控制不变集范围内的受干扰的最大程度的优化问题

x = sdpvar(2,1); % 定义优化变量
% 约束是基于给定的特征矩阵P、mu、wmax​和lambda0。这个约束实际上是在确保x属于robust invariant set的前提下进行优化
% 见论文Lemma 2.
const = x' * P * x <= (mu * wmax^2 / lambda0); % 约束条件
% const = x' * P1 * x <= 1;
obj1 = K * x; % 目标函数
% obj2 = [1, 0] * x;
% obj3 = [0, 1] * x;
% 最小化目标函数obj1，并且满足约束条件const
optimize(const, obj1); % 优化
% 得到优化后的目标函数的最小值，即得到了控制输入u的最优值
u_0 = value(obj1); % 获取优化结果

% optimize(const, obj2);
% x10 = value(obj2);
% optimize(const, obj3);
% x20 = value(obj3);

%% Get the terminal region Xf and terminal penalty for nominal MPC=终端最大鲁棒正不变集

% 计算了终端区域Xf​和终端惩罚Pnom​的最大值，以及控制增益K0nom。这些参数是在MPC框架中使用
[P_nom, K0_nom, alpha_nom] = NMPC_get_max_terminal_Tube(Q, R, uM + u_0, xM, alphaM);
% 论文中提供的一组硬编码的参数值，用于对比和验证的标准值
P_nom1 = [7.9997, -12.2019; -12.2019, 27.0777];
% pause(1)
% 绘制以Pnom​为矩阵的椭圆，用绿色点线表示。这个椭圆是终端区域Xf​的表示

% 图1: 终端不变集范围
draw_ellip(P_nom, alpha_nom, 'g.')
hold on
% 红色点线表示。这个椭圆是用于对比的标准值
draw_ellip(P_nom1, alpha_nom, 'r.')

%% MPC Optimization problem using YALMIP
for v = 1 : 1 % 1 : 6 在不同扰动下收集数据的循环
    x0 = [3.5; -2.5]; % 初始状态 x0
    xc = zeros(2, T); % 存储实际状态
    uc = zeros(1, T);
    xn = zeros(2, T); % 存储名义状态
    un = zeros(1, T);
    tilde_x = zeros(2, T); % 存储实际状态-名义状态的差
    xc(:, 1) = x0;
    xn(:, 1) = x0;
    w = zeros(1, T);
    for i = 1 : T-(N-1)
        % 定义决策变量x和u为优化变量
        x = sdpvar(2, N);
        u = sdpvar(1, N-1);
        w(i) = pick_random_disturbance(W); % 从扰动集合中随机选择扰动
        % w(i) = 0.1;

        % 定义约束
        const = [u <= (uM + u_0), u >= (-uM - u_0), x(:,1) == x0];
        % const = [u <= 2, u >= -2, x(:,1) == x0];
        for k = 1 : N-1
            const = [const, x(:,k+1) == x(:, k) + dt * ( A * x(:, k) + B * u(k) + [0; -0.25 * x(2, k)^3] )];
        end
        const = [const, x(:, N)' * P_nom * x(:, N) <= alpha_nom]; % x ∈ Xf:terminal set

        % 定义目标
        obj = 0;
        for j = 1 : N-1
            obj = obj + x(:, j)' * Q * x(:, j) + u(j)' * R * u(j); % stage cost
        end
        obj = obj + x(:, N)' * P_nom * x(:, N); % terminal penalty function

        % 优化
        optimize(const, obj);
        %optimize(const, obj, sdpsettings('solver','fmincon','fmincon.maxiter',3000));

        % 控制和更新
        un(i) = value(u(1));
        uc(i) = un(i) + K * tilde_x(:, i);
        xn(:, i+1) = xn(:, i) + dt * ( A * xn(:, i) + B * un(i) + [0; -0.25 * xn(2, i)^3] );
        xc(:, i+1) = xc(:, i) + dt * ( A * xc(:, i) + B * uc(i) + [0; -0.25 * xc(2, i)^3] + Bw * w(i));
        tilde_x(:, i+1) = xc(:, i+1) - xn(:, i+1);
        x0 = xn(:, i+1);
        %x0 = value(x(:,2));

        i % 显示当前步数
        
        % 迭代绘制每一步的状态tube范围
        if v == 1
            % 图4 : 绘制椭圆，表示状态偏差的不确定性
            % P : 椭圆的协方差矩阵，表示椭圆的形状和方向。
            % mu * wmax^2/lambda0 : 椭圆的半长轴长度，通过对参数进行线性组合计算得到。这个值决定了椭圆的大小。
            % [xn(1,i), xn(2,i)] : 椭圆的中心位置，即状态偏差的当前值。
            % 'y' : 椭圆的颜色，这里是黄色。
            figure(4)
            draw_ellip2(P, mu * wmax^2/lambda0, [xn(1,i), xn(2,i)] ,'y')
            hold on
        end
    end
    
    for i = T-N+2 : T-1
        w(i) = pick_random_disturbance(W); % 从扰动集合中随机选择扰动
        % w(i) = 0.1;
        tilde_x(:, i) = xc(:, i) - xn(:, i);
        un(i) = value(u(i-T+N));
        xn(:, i+1) = value(x(:,i-T+N)); % ??
        uc(i) = un(i) + K * tilde_x(:, i);
        xc(:, i+1) = xc(:, i) + dt * ( A * xc(:, i) + B * uc(i) + [0; -0.25 * xc(2, i)^3] + Bw * w(i));
    end
    tilde_x(:, T) = xc(:, T) - xn(:, T);
    
    % Save the data under different disturbances
    % pathname = 'F:\实验室\仿真\MPC\Reproduction of TMPC\tube-based test data set\';
    % str0 = num2str(v);
    % filename1 = ['xc',str0];
    % filename2 = ['xn',str0];
    % filename3 = ['uc',str0];
    % filename4 = ['un',str0];
    % save([pathname,filename1],'xc');
    % save([pathname,filename2],'xn');
    % save([pathname,filename3],'uc');
    % save([pathname,filename4],'un');

    t = 0: dt: (T-1) * dt;
    t_com = 0 : dt: (N-1) * dt;

    % 图2: 实际状态轨迹和标称状态轨迹对比
    figure(2)
    plot(t, xc(1,:), 'k', t, xc(2,:), 'b')
    hold on
    plot(t, xn(1,:), 'r', t, xn(2,:), 'g')
    hold on
    
    % 图3: 实际输入和标称输入对比
    figure(3)
    plot(t, uc(:))
    hold on
    plot(t, un(:),'r')
    hold on
    
    % 图4: 绘制终端状态tube范围/绘制实际状态轨迹和标称状态轨迹
    figure(4)
    draw_ellip(P_nom, alpha_nom, 'g')
    hold on
    % plot(Omega)
    % hold on
    % draw_ellip(P, mu * 0.01/lambda, 'y')
    % hold on
    plot(xn(1, :),xn(2, :),'-*r')
    hold on
    plot(xc(1, :),xc(2, :),'-*')
    hold on
    % xlim([-4,4])
    % ylim([-5,2])
    
    % 图5: 实际状态轨迹和标称状态轨迹之差
    figure(5)
    plot(t, tilde_x(1,:), 'k', t, tilde_x(2,:), 'b')
    hold on    
    
    % 图6: 实际状态轨迹和标称状态轨迹之差/鲁棒不变集的范围
    figure(6)
    plot(tilde_x(1,:), tilde_x(2,:), 'b*')
    hold on
    draw_ellip2(P, mu * wmax^2/lambda0, [0, 0] ,'r.')
end

%% Plot the trajectory
% t = 0: dt: (T-1) * dt;
% t_com = 0 : dt: (N-1) * dt;
% figure(1)
% plot(t, xc(1,:), 'k', t, xc(2,:), 'b')
% hold on
% plot(t, xn(1,:), 'r', t, xn(2,:), 'g')
% 
% figure(2)
% plot(t, uc(:))
% hold on
% plot(t, un(:),'r')
% 
% figure(3)
% draw_ellip(P_nom, alpha_nom, 'g')
% hold on
% % plot(Omega)
% % hold on
% % draw_ellip(P, mu * 0.01/lambda, 'y')
% % hold on
% plot(xn(1, :),xn(2, :),'-*r')
% hold on
% plot(xc(1, :),xc(2, :),'-*')
% % xlim([-4,4])
% % ylim([-5,2])
% 
% figure(4)
% plot(t, tilde_x(1,:), 'k', t, tilde_x(2,:), 'b')
% hold on
% 
% figure(5)
% plot(tilde_x(1,:), tilde_x(2,:), 'b*')
% hold on
% draw_ellip2(P, mu * wmax^2/lambda, [0, 0] ,'r.')